﻿using System;

namespace Raven
{
    public static class Geometry
    {
        //-------------------- LineIntersection2D-------------------------
        //
        //	Given 2 lines in 2D space AB, CD this returns true if an 
        //	intersection occurs and sets dist to the distance the intersection
        //  occurs along AB. Also sets the 2d vector point to the point of
        //  intersection
        //----------------------------------------------------------------- 
        public static bool LineIntersection2D(Vector2 A,
            Vector2 B,
            Vector2 C,
            Vector2 D,
            ref Fix dist,
            ref Vector2 point)
        {
            Fix rTop = (A.y - C.y) * (D.x - C.x) - (A.x - C.x) * (D.y - C.y);
            Fix rBot = (B.x - A.x) * (D.y - C.y) - (B.y - A.y) * (D.x - C.x);

            Fix sTop = (A.y - C.y) * (B.x - A.x) - (A.x - C.x) * (B.y - A.y);
            Fix sBot = (B.x - A.x) * (D.y - C.y) - (B.y - A.y) * (D.x - C.x);

            if ((rBot == 0) || (sBot == 0))
            {
                //lines are parallel
                return false;
            }

            Fix r = rTop / rBot;
            Fix s = sTop / sBot;

            if ((r > 0) && (r < 1) && (s > 0) && (s < 1))
            {
                dist = (A - B).magnitude * r;

                point = A + r * (B - A);

                return true;
            }

            else
            {
                dist = 0;

                return false;
            }
        }

        //------------------------- DistToLineSegment ----------------------------
        //
        //  given a line segment AB and a point P, this function calculates the 
        //  perpendicular distance between them
        //------------------------------------------------------------------------
        public static Fix DistToLineSegment(Vector2 A,
            Vector2 B,
            Vector2 P)
        {
            //if the angle is obtuse between PA and AB is obtuse then the closest
            //vertex must be A
            Fix dotA = (P.x - A.x) * (B.x - A.x) + (P.y - A.y) * (B.y - A.y);

            if (dotA <= 0) return (A - B).magnitude;

            //if the angle is obtuse between PB and AB is obtuse then the closest
            //vertex must be B
            Fix dotB = (P.x - B.x) * (A.x - B.x) + (P.y - B.y) * (A.y - B.y);

            if (dotB <= 0) return (B - P).magnitude;

            //calculate the point along AB that is the closest to P
            Vector2 Point = A + ((B - A) * dotA) / (dotA + dotB);

            //calculate the distance P-Point
            return (P - Point).magnitude;
        }

        public static bool LineIntersection2D(Vector2 A,
            Vector2 B,
            Vector2 C,
            Vector2 D)
        {
            Fix rTop = (A.y - C.y) * (D.x - C.x) - (A.x - C.x) * (D.y - C.y);
            Fix sTop = (A.y - C.y) * (B.x - A.x) - (A.x - C.x) * (B.y - A.y);

            Fix Bot = (B.x - A.x) * (D.y - C.y) - (B.y - A.y) * (D.x - C.x);

            if (Bot == 0) //parallel
            {
                return false;
            }

            Fix r = rTop / Bot;
            Fix s = sTop / Bot;

            if ((r > 0) && (r < 1) && (s > 0) && (s < 1))
            {
                //lines intersect
                return true;
            }

            //lines do not intersect
            return false;
        }

        //------------------ isSecondInFOVOfFirst -------------------------------------
        //
        //  returns true if the target position is in the field of view of the entity
        //  positioned at posFirst facing in facingFirst
        //-----------------------------------------------------------------------------
        public static bool isSecondInFOVOfFirst(Vector2 posFirst,
            Vector2 facingFirst,
            Vector2 posSecond,
            Fix fov)
        {
            Vector2 toTarget = (posSecond - posFirst).normalized;

            return Vector2.Dot(facingFirst, toTarget) >= FixMath.Cos(fov / Fix.Two);
        }

        //--------------------- LineSegmentCircleIntersection ---------------------------
        //
        //  returns true if the line segemnt AB intersects with a circle at
        //  position P with radius radius
        //------------------------------------------------------------------------
        public static bool LineSegmentCircleIntersection(Vector2 A,
            Vector2 B,
            Vector2 P,
            Fix radius)
        {
            //first determine the distance from the center of the circle to
            //the line segment (working in distance squared space)
            Fix DistToLineSq = DistToLineSegmentSq(A, B, P);

            if (DistToLineSq < radius * radius)
            {
                return true;
            }

            return false;
        }

        //------------------------- DistToLineSegmentSq ----------------------------
        //
        //  as above, but avoiding sqrt
        //------------------------------------------------------------------------
        public static Fix DistToLineSegmentSq(Vector2 A,
            Vector2 B,
            Vector2 P)
        {
            //if the angle is obtuse between PA and AB is obtuse then the closest
            //vertex must be A
            Fix dotA = (P.x - A.x) * (B.x - A.x) + (P.y - A.y) * (B.y - A.y);

            if (dotA <= 0) return (A - P).sqrMagnitude;

            //if the angle is obtuse between PB and AB is obtuse then the closest
            //vertex must be B
            Fix dotB = (P.x - B.x) * (A.x - B.x) + (P.y - B.y) * (A.y - B.y);

            if (dotB <= 0) return (B - P).sqrMagnitude;

            //calculate the point along AB that is the closest to P
            Vector2 Point = A + ((B - A) * dotA) / (dotA + dotB);

            //calculate the distance P-Point
            return (P - Point).sqrMagnitude;
        }

        //------------------- GetLineSegmentCircleClosestIntersectionPoint ------------
        //
        //  given a line segment AB and a circle position and radius, this function
        //  determines if there is an intersection and stores the position of the 
        //  closest intersection in the reference IntersectionPoint
        //
        //  returns false if no intersection point is found
        //-----------------------------------------------------------------------------
        public static bool GetLineSegmentCircleClosestIntersectionPoint(Vector2 A,
            Vector2 B,
            Vector2 pos,
            Fix radius,
            ref Vector2 IntersectionPoint)
        {
            Vector2 toBNorm = (B - A).normalized;

            //move the circle into the local space defined by the vector B-A with origin
            //at A
            Vector2 LocalPos = Transformations.PointToLocalSpace(pos, toBNorm, toBNorm.Perp(), A);

            bool ipFound = false;

            //if the local position + the radius is negative then the circle lays behind
            //point A so there is no intersection possible. If the local x pos minus the 
            //radius is greater than length A-B then the circle cannot intersect the 
            //line segment
            if (LocalPos.x + radius >= 0 &&
                (LocalPos.x - radius) * (LocalPos.x - radius) <= (B - A).sqrMagnitude)
            {
                //if the distance from the x axis to the object's position is less
                //than its radius then there is a potential intersection.
                if (FixMath.Abs(LocalPos.y) < radius)
                {
                    //now to do a line/circle intersection test. The center of the 
                    //circle is represented by A, B. The intersection points are 
                    //given by the formulae x = A +/-sqrt(r^2-B^2), y=0. We only 
                    //need to look at the smallest positive value of x.
                    Fix a = LocalPos.x;
                    Fix b = LocalPos.y;

                    Fix ip = a - FixMath.Sqrt(radius * radius - b * b);

                    if (ip <= 0)
                    {
                        ip = a + FixMath.Sqrt(radius * radius - b * b);
                    }

                    ipFound = true;

                    IntersectionPoint = A + toBNorm * ip;
                }
            }

            return ipFound;
        }
    }
}